#include <iostream>
#include <cmath>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>


using namespace std;


float atan3(float,float);
float ShiftPosition(float);
float DetectorPosition(float , float, float, float );

class ScanData{
public:
	int Ng, Nt;
	float gmin, gmax, tmin, tmax, ti, tf, dg, dt, R, theta,rov;
	vector<vector<float> > data;
	vector<vector<float> > Ddata;
	ScanData(int,float,float, int, float, float, float, float);
	vector<vector<float> > backproj( int , float , float , int , float , float );

};


float IndexPosition( const ScanData& , const int );
int PositionIndex(const ScanData&, const float, bool);
bool isMissing(const ScanData& , const float);
bool isHilbertKnown(const ScanData&, float , float );
float DFint(const ScanData&, int , float);
void project(ScanData&, float&, int, int, float, float, bool);

ScanData::ScanData (int ng ,float gmi ,float gma, int nt, float tmi, float tma, float the,  float rad)

{
	Ng = ng;
	Nt = nt;
	gmin = gmi;
	gmax= gma;
	ti = tmi;
	tf = tma;
	tmin = ShiftPosition(tmi+the);
	tmax = ShiftPosition(tma+the);
	theta = the;
	R = rad;
	dg=(gmax-gmin)/(Ng-1.);
	dt=2.*M_PI/984.; //to be changed !!
	cout<<dt<<endl;
	rov = R*sin((gmax - gmin)/2.);	
	data = vector<vector<float> >  (Nt, vector<float> (Ng));
	Ddata = vector<vector<float> >  (Nt, vector<float> (Ng));
	
}


float DFint(const ScanData & s, int p, float G){
	//if(p>=s.Nt||p<0) return 0;
	float w1,w2, G1,G2;
	int m1,m2;
	
	m1 = (int) floor((G-s.gmin)/s.dg);
	m2 = (int) ceil((G-s.gmin)/s.dg);
	G1 = s.gmin + s.dg*m1;
	G2 = s.gmin + s.dg*m2;
	if(m1==m2){
		w1=0.5;
		w2=0.5;
	}else{
		w1 = (G - G2)/(G1-G2);
		w2 = (G - G1)/(G2-G1);
	}

	//cout<< m1 << " " << m2 << endl;	
	if (m2 < 0 || m2>=s.Ng){
		return (s.Ddata[p][m1]);
	}else if(m1 < 0 || m1>=s.Ng){
		return (s.Ddata[p][m2] );
	}else{
		return w1*(s.Ddata[p][m1]) +
			w2*(s.Ddata[p][m2] );
	}	
		
}

float Fint(const ScanData & s, int p, float G){
	float w1,w2;
	int m1,m2;
	//M[0] = floor(U/du + (float)(Nu-1)/2.); // lower index
	m1 = (int) floor((G-s.gmin)/s.dg);
	//M[1] = ceil(U/du + (float)(Nu-1)/2.);
	m2 = (int) ceil((G-s.gmin)/s.dg);
	//cout<< M[0] << " " << M[1] << "\n";
	//float U0 = du*((float) M[0] - (Nu-1)/2.);
	//float U1 = du*((float) M[1] - (Nu-1)/2.);
	float G0 = s.gmin + s.dg*((float)m1);
	float G1 = s.gmin + s.dg*((float)m2);
	
	if(m1==m2){
		w1=0.5;
		w2=0.5;
	}else{
		w1 = (G - G1)/(G0-G1);
		w2 = (G - G0)/(G1-G0);
	}

	if (m2 < 0 || m2>=s.Ng){
		return (s.data[p][m1]);
	}else if(m1 < 0 || m1>=s.Ng){
		return (s.data[p][m2] );
	}else{
		return w1*(s.data[p][m1]) +
			w2*(s.data[p][m2] );
	}	
	//cout<< M[0] << " " << M[1] << endl;		

}
		


vector<vector<float> > ScanData::backproj( int Nx, float xmin, float xmax, int Ny, float ymin, float ymax)
// returns the backprojected data.
{
	int p=0; // t index
	int n=0; // u index
	int i=0; // x index
	int j=0; // y index
	int pa=0,pb=0,temp=0, pi=0,p1=0, pf=0;
	float x=0,y=0,t=0,L=0,G=0,ta=0,tb=0,phi=0,rev=1;
	float dx= (xmax-xmin)/(Nx-1.);
	float dy= (ymax-ymin)/(Ny-1.);
	vector<vector<float> >  image (Nx, vector<float> (Ny,0));


	// Differentiate
	//Gamma derivative
	for(p=0; p<Nt;++p){ // t loop
		Ddata[p][0] = (-3./2.*data[p][0] + 2.*data[p][1] - 1./2.*data[p][2])/dg;
		for(n=1; n<Ng-1;++n){ // g loop
			Ddata[p][n] = (-1./2.*data[p][n-1] + 1./2.*data[p][n+1])/dg;
		}
		Ddata[p][Ng-1] = (3./2.*data[p][Ng-1] - 2.*data[p][Ng-2] + 1./2.*data[p][Ng-3])/dg;
	}
	
	//t derivative
	
	for(n=0; n<Ng;++n){ // g loop
		Ddata[0][n] = (-3./2.*data[0][n] + 2.*data[1][n] - 1./2.*data[2][n])/dt - Ddata[0][n];
		for(p=1; p<Nt-1;++p){ // t loop
			Ddata[p][n] = (-1./2.*data[p-1][n] + 1./2.*data[p+1][n])/dt - Ddata[p][n];
		}
		Ddata[Nt-1][n] = (3./2.*data[Nt-1][n] - 2.*data[Nt-2][n] + 1./2.*data[Nt-3][n])/dt - Ddata[Nt-1][n];
	}
	
	// Perform backprojection
	for(j=0; j<Ny;++j){ // y loop
		y = ymin + dy * j;
		
		ta = ShiftPosition( asin(y/R) );
		G = DetectorPosition(0, y, ta, R);
		if(ta>=0){ 
			tb = ShiftPosition(ta+2.*G+M_PI);
		}else{ 
			tb = ShiftPosition(ta+2.*G+M_PI);
		}
		pi = (int) ceil(ShiftPosition(ta-tmin)/dt);
		cout<< pi << " "<<ta<<" "<<tmin<<endl;
		
		pi = (int) PositionIndex(*this ,ta,true);
		pf = (int) PositionIndex(*this ,tb,false);
		//cout <<j <<", "<< ta <<", "<< IndexPosition(*this,pi) <<", "<<  tb << ", " << IndexPosition(*this,pf) << ", "<< pi << ", " <<pf << endl;
		for(i=0; i<Nx;++i){ // x loop
			x = xmin + dx * (i);

			if (isHilbertKnown(*this, x, y)){

				if(isMissing(*this,ta)){
					project(*this, image[i][j], 0,  pf,  x,  y, false);
					pi = (int) ceil((ta-tmin)/dt);
					project(*this, image[i][j], pi,  -1,  x,  y, true);
				}else if(isMissing(*this,tb)){
					project(*this, image[i][j], pi,  Nt-1,  x,  y, false);
					if(tb<0) tb = tb + 2.* M_PI;
					pf = (int) ceil((tb-tmax)/dt);
					project(*this, image[i][j], Nt,  pf+Nt-1,  x,  y, true);
				}else{
					if(pi>pf){
						project(*this, image[i][j], pi,  Nt-1,  x,  y, false);
						p1 = (int) floor((tmin-tmax)/dt); //might need to be improved
						project(*this, image[i][j], Nt,  Nt+p1-1,  x,  y, true);
						project(*this, image[i][j], 1,  pf,  x,  y, false);
					}else{
						project(*this, image[i][j], pi,  pf,  x,  y, false);
					}
				}
			}else{
				image[i][j] = 0;
			}
		}
	}
	return image;
}

vector<vector<float> > filter(const ScanData & s, vector<vector<float> > & BP,int Nx, float xmin, float xmax, int Ny, float ymin, float ymax)
// returns the filtered backprojected data.
{

	int i=0; // x index
	int j=0; // y index
	int n=0; // convolution index
	int na, nb, pa, pb, imin, imax;

	float x=0, y=0, ta, tb;
	float dx= (xmax-xmin)/(Nx+1.);
	float dy= (ymax-ymin)/(Ny+1.);
	vector<vector<float> >  image (Nx, vector<float> (Ny,0));
	vector<float> C (Ny,0);
	
	float r = s.R*sin((s.gmax - s.gmin)/2.);

	// pre weight
	for(j=0; j<Ny;++j){ // t loop
		y = ymin + dy * (j);
		for(i=0; i<Nx;++i){ // g loop
			x = xmin + dx * (i);
			if (x*x + y*y < r*r) BP[i][j] = sqrt(r*r - y*y - x*x)*BP[i][j];
		}
		
	}
	
	// convolution
	
	for(j=0; j<Ny;++j){ // y loop
		y = ymin + dy * j;
		imin = (int) ceil((-sqrt(r*r - y*y)- xmin )/dx);
		imax = (int) floor((sqrt(r*r - y*y)- xmin )/dx);	
		for(i=imin; i<=imax;++i){ // x loop
			for(n=imin; n<=imax;++n){ // x prime loop
				if( ((i-n)%2 != 0)||( i-n != 0) ) image[i][j] = image[i][j] + BP[n][j] * (-2./(n-i));
			}
			
		}
		// Compute C
		
		
		ta = ShiftPosition(fmod((double) asin(y/s.R) + 2.*M_PI, 2.*M_PI));
		pa = (int) ceil(fmod((double)(ta - s.tmin)/s.dt + 2.*s.Nt,s.Nt));
		//na = floor((ta - s.gmin)/s.dg);
		if(ta>=0){ 
			tb = ShiftPosition(fmod((double) M_PI - ta + 2.*M_PI, 2.*M_PI));
		}else{ 
			tb = ShiftPosition(fmod((double) M_PI + ta + 2.*M_PI, 2.*M_PI));
		}
		pb = (int) floor(fmod((double)(tb - s.tmin)/s.dt + 2.*s.Nt,s.Nt));
		//nb = ceil((-ta - s.gmin)/s.dg);
		//C[j] = -M_PI*(s.data[pa][na] + s.data[pb][nb]);
		C[j] = -2*M_PI*(Fint(s, pa,-ta) + Fint(s, pb, ta));
	}
	
	
	// post weight
	for(j=0; j<Ny;++j){ // t loop
		y = ymin + dy * j;
		for(i=0; i<Nx;++i){ // g loop
			x = xmin + dx * (i);
			if (x*x + y*y < r*r){
				image[i][j] = -0.5/pow(M_PI,2)/sqrt(r*r - y*y - x*x)*( image[i][j] + C[j] );
			}else{
				image[i][j] = 0;
			}
		}
	}
	
	return image;
}


float atan3(float y,float x)
{
	float a = atan2(y,x);
	if (a>=0){ return a;}
	else{ return 2.*M_PI+a;}
}

float IndexPosition(const ScanData &s, const int p){
	return ShiftPosition(s.tmin + s.dt * p);
}

int PositionIndex(const ScanData &s, const float t, bool isCeil){
	float T =(t-s.tmin);
	if ((ShiftPosition(t)<s.tmin||ShiftPosition(t)>s.tmin+M_PI)&&!(s.tmin<0 && t>ShiftPosition(M_PI+s.tmin))) T= 2.*M_PI+ T;
	
	int p;
	if(isCeil){p = (int) ceil(fmod((double)(T)/s.dt + 8.*(s.Nt),s.Nt));}
	else{p = (int) floor(fmod((double)(T)/s.dt + 8.*(s.Nt),s.Nt));}
	return p;
}

float ShiftPosition(const float t){
	float T = fmod((double) t + 9.*M_PI,2.*M_PI);
	return T - M_PI;
}

float DetectorPosition(float x, float y, float t, float R)
// Returns the detector position (gamma) from ray passing through x,y and source position t.
{	
	float phi = atan3(y-R*sin(t) , x-R*cos(t) );
	if( t<=M_PI && t>=M_PI_2 && phi < M_PI ){
		return phi - t + M_PI;
	}else if(t>-M_PI && t<=-M_PI_2 && phi>=M_PI ){
		return phi - t - 3.* M_PI;
	}else{
		return phi - t - M_PI;
	}
}

bool isMissing(const ScanData & s, const float t)
// Returns true if data at source position t is not available
{
	if(s.tmin < 0 && s.tmax < 0){
		if(s.tmin > s.tmax && t>s.tmax && t<s.tmin){ return true;}
		else if(s.tmin < s.tmax && (t>s.tmax || t<s.tmin)){ return true;}
		else {return false;}
	}else if(s.tmin >= 0 && s.tmax >= 0){
		if(s.tmin > s.tmax && t>s.tmax && t<s.tmin){ return true;}
		else if(s.tmin < s.tmax && (t>s.tmax || t<s.tmin)){ return true;}
		else {return false;}
	}else if(s.tmin>=0 && s.tmax<0){
		if(t>s.tmax && t<s.tmin){return true;}
		else{return false;}
	}else if(s.tmin< 0 && s.tmax>=0){
		if(t>s.tmax || t<s.tmin){return true;}
		else{return false;}
	}else{
		return false;
	}
}


bool isHilbertKnown(const ScanData & s, float x, float y)
// Returns true if the correct backprojection can be obtained at point x,y.
{
	if (x*x + y*y < s.rov*s.rov){ //return true;
		float medang = (s.tmin + s.tmax)/2.;
		if(s.tmax>s.tmin) medang = medang + M_PI;

		float k[2] = {cos(medang), sin(medang)};
		float C = k[0]*s.R*cos(s.tmax) + k[1]*s.R*sin(s.tmax);

		if(k[0]*x + k[1]*y<C){return true;}
		else{return false;}
	}else{
		return false;
	}
}

void project(ScanData & s, float & image, int pi, int pf, float x, float y, bool isConj){
	int p=0,p1=0;
	float t, L, G;
	if(isConj){
		for(p=pi;p<=pf;p++){
			t = ShiftPosition(s.tmin + p*s.dt);
			L = sqrt( pow(x - s.R*cos(t),2) + pow(y - s.R*sin(t),2) );
			G = DetectorPosition(x, y, t, s.R);			
			t = ShiftPosition(t + 2.*G + M_PI);
			p1 = PositionIndex(s ,t,true);
			image = image - s.dt/L * DFint(s, p1,-G);
		}
	}else{
		for(p=pi; p<=pf;++p){
			t = IndexPosition(s,p); 
			L = sqrt( pow(x - s.R*cos(t),2) + pow(y - s.R*sin(t),2) );
			G = DetectorPosition(x, y, t, s.R);
			image = image + s.dt/L * DFint(s, p, G);
		}
	}
}
// Returns the source position t of the conjugate ray w.r.t T and G.
{
	
	return 0.;
};

int main (int argc, char* argv[]) 
{
	int i=0,j=0;
	//char c;
	vector<vector<float> > bp;
	vector<vector<float> > F;
	//double f = 54.889599/2.*348./888.; //Trunc

	double f = 54.889599/2.;
	
	ScanData s = ScanData (888,-f/180.*M_PI , f/180.*M_PI, 575, M_PI*(2.-1./12.), M_PI*(1.+1./12.), M_PI_4, 541 ); //testShortScan.csv
	//ScanData s = ScanData (888,-f/180.*M_PI , f/180.*M_PI, 984, 3.*M_PI_2, 3.*M_PI_2, -M_PI_4, 541 ); //testSS.csv
	//ScanData s = ScanData (888,-f/180.*M_PI , f/180.*M_PI, 984, 0, 2.*M_PI, M_PI_2, 541 ); // trunc
	ifstream file ("data/testShortScan.csv");
	//ifstream file ("data/testSS.csv");
	if (file.is_open())
	{
		file.precision(20);
		
		string line;
		while (getline (file, line))
		{
			istringstream linestream(line);
			string item;
			while (getline (linestream, item, ','))
			{
				s.data[j][i]  = (float) atof( item.c_str() );
				j++;
			}
			j=0;
			i++;
		}
		
		file.close();
		
	
		int Nx=512;
		int Ny=512;
		int max=2*(128+64);
		bp= s.backproj( Nx, -max, max, Ny, -max, max);
		//F= filter(s, bp, Nx, -max, max, Ny, -max, max);

		
		
		// write to file
		ofstream bpfile ("reconTrunc.csv",ios::trunc);
		bpfile.precision(20);
		for(i=0;i<Nx;++i){
			for(j=0;j<Ny;++j){
				//bpfile << F[i][j] << ",";
				bpfile << bp[i][j] << ",";
			}
			bpfile << "\n";
		}
	
		bpfile.close();
	}else{
		cout << "Data file error.\n";
	}/*
	double t,dt=2.*M_PI/99;
	for(i=0;i<100;i++){
		
		t = -M_PI_2 + i*dt;
		cout << t << ": "<< ShiftPosition(t)<<endl;	
	}*/
}
